function draw_rec_animation(path)
hold on;
axis equal;
for i = 1:length(path)
    rectangle('Position', [path(i).position(1,1),path(i).position(1,2),1,1], 'FaceColor', [0 .5 .5]);
    %line([path(i).position(1,1) + 0.5, path(i+1).position(1,1) + 0.5],...
         %[path(i).position(1,2) + 0.5, path(i+1).position(1,2) + 0.5],'Color', 'r');
    rectangle('Position', [path(i).position(1,1) + 0.25,path(i).position(1,2) + 0.25,0.5,0.5], 'Curvature',[1 1], ...
              'FaceColor', 'r','EdgeColor',[0 .5 .5]);
    pause(0.1);
    rectangle('Position', [path(i).position(1,1) + 0.25,path(i).position(1,2) + 0.25,0.5,0.5], 'Curvature',[1 1], ...
              'FaceColor', [0 .5 .5],'EdgeColor',[0 .5 .5]);
end
rectangle('Position', [path(end).position(1,1) + 0.25,path(end).position(1,2) + 0.25,0.5,0.5], 'Curvature',[1 1], ...
              'FaceColor', 'r','EdgeColor',[0 .5 .5]);